/*
 * nvic.c
 *
 *  Created on: Dec 7, 2023
 *      Author: boen
 */

/**
  * @brief  Tx Transfer completed callbacks.
  * @param  huart: pointer to a UART_HandleTypeDef structure that contains
  *                the configuration information for the specified UART module.
  * @retval None
  */
#include <nvic.h>

extern int debug_get_msg(uint8_t buffer[DEBUG_RX_BUFF_MAX]);
extern int serial_servo_get_reply(uint8_t buffer[SERVO_RX_BUFF_MAX]);

#if __CAN_1_ENB__
CAN_TxHeaderTypeDef        Tx1Message;    	// CAN1消息发送结构
CAN_RxHeaderTypeDef        Rx1Message;    	// CAN1消息接收结构
#endif

#if __CAN_2_ENB__
CAN_TxHeaderTypeDef        Tx2Message;    	// CAN2消息发送结构
CAN_RxHeaderTypeDef        Rx2Message;    	// CAN2消息接收结构
#endif


typedef struct{
	uint8_t rx_buffer[SERVO_RX_BUFF_MAX];
}QServoMsg_t;

QServoMsg_t servo_qmsg;

uint8_t servo_rx_buffer[SERVO_RX_BUFF_MAX];
uint8_t debug_rx_buffer[DEBUG_RX_BUFF_MAX];
uint8_t imu_rx_buffer[IMU_RX_BUFF_MAX];

/**
  * @brief Tx Transfer completed callbacks
  * @param huart: uart handle
  * @retval None
  */
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
	if(huart->Instance == SERVO_PORT_ID)
	{
		HAL_GPIO_WritePin(SERIAL_SERVO_RX_EN_GPIO_Port, SERIAL_SERVO_RX_EN_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(SERIAL_SERVO_TX_EN_GPIO_Port, SERIAL_SERVO_TX_EN_Pin, GPIO_PIN_SET);
		if(HAL_UART_Receive_IT(&SERVO_PORT, servo_rx_buffer, ONEBYTE) != HAL_OK)
		{
			//printf("HAL_UART_TxCpltCallback error[0].\r\n");
		}
	}
}

/**
  * @brief Rx Transfer completed callbacks
  * @param huart: uart handle
  * @retval None
  */
static int rec_servo_step = APP_NULL;
static int rec_imu_step = APP_NULL;

#if DETA10
static uint8_t imu_yaw_byteArry[4];
#endif

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	if(huart->Instance == SERVO_PORT_ID)
	{
		switch(rec_servo_step)
		{
			case 0:
				if(servo_rx_buffer[0] == 0x55u)
				{
					HAL_UART_Receive_IT(&SERVO_PORT, servo_rx_buffer+ONEBYTE, (8-ONEBYTE));
					rec_servo_step = 1;
				}
				else
				{
					HAL_UART_Receive_IT(&SERVO_PORT, servo_rx_buffer, ONEBYTE);
				}
				break;
			case 1:
//				XDEBUG("[dbg]: servo rx completed.\r\n");
//				XDEBUG("rxd:[%02x, %02x, %02x, %02x, %02x, %02x, %02x, %02x]\r\n",servo_rx_buffer[0],
//						servo_rx_buffer[1],
//						servo_rx_buffer[2],
//						servo_rx_buffer[3],
//						servo_rx_buffer[4],
//						servo_rx_buffer[5],
//						servo_rx_buffer[6],
//						servo_rx_buffer[7]);
				serial_servo_get_reply(servo_rx_buffer);
				rec_servo_step = 0;
				break;
		}
	}

	if(huart->Instance == IMU_PORT_ID)
	{
#if JY901S
		switch(rec_imu_step)
		{
			case 0:
				if(imu_rx_buffer[0] == 0x55)
				{
					rec_imu_step = 1;
					HAL_UART_Receive_IT(&IMU_PORT, imu_rx_buffer+1, ONEBYTE);
				}
				else
				{
					HAL_UART_Receive_IT(&IMU_PORT, imu_rx_buffer, ONEBYTE);
				}
				break;
			case 1:
				if(imu_rx_buffer[1] == 0x53)
				{
					rec_imu_step = 2;
					HAL_UART_Receive_IT(&IMU_PORT, imu_rx_buffer+2, (IMU_RX_BUFF_MAX-3));
				}
				else
				{
					rec_imu_step = 0;
					HAL_UART_Receive_IT(&IMU_PORT, imu_rx_buffer, ONEBYTE);
				}
				break;
			case 2:
				float yaw;
				int tmp = 0;
				tmp = imu_rx_buffer[7];
				tmp  <<= 8;
				tmp += imu_rx_buffer[6];
				yaw = tmp;
				yaw = yaw/32768*180;
				imu_get_reply(deg2rad(yaw));
				rec_imu_step = 0;
				HAL_UART_Receive_IT(&IMU_PORT, imu_rx_buffer, ONEBYTE);
				break;
		}
#endif

#if DETA10
		switch(rec_imu_step)
		{
			case 0:
				if(imu_rx_buffer[0] == 0xFC)
				{
					rec_imu_step = 1;
					HAL_UART_Receive_IT(&IMU_PORT, imu_rx_buffer+1, (2));
				}
				else
				{
					HAL_UART_Receive_IT(&IMU_PORT, imu_rx_buffer, ONEBYTE);
				}
				break;
			case 1:
				if((imu_rx_buffer[1] == 0x41)&&(imu_rx_buffer[2] == 0x30))
				{
					rec_imu_step = 2;
					HAL_UART_Receive_IT(&IMU_PORT, imu_rx_buffer+3, (IMU_RX_BUFF_MAX-3));
				}
				else
				{
					rec_imu_step = 0;
					HAL_UART_Receive_IT(&IMU_PORT, imu_rx_buffer, ONEBYTE);
				}
				break;
			case 2:
				imu_yaw_byteArry[0] = imu_rx_buffer[20+7];
				imu_yaw_byteArry[1] = imu_rx_buffer[21+7];
				imu_yaw_byteArry[2] = imu_rx_buffer[22+7];
				imu_yaw_byteArry[3] = imu_rx_buffer[23+7];
				imu_get_reply(byte2float(imu_yaw_byteArry));
				rec_imu_step = 0;
				HAL_UART_Receive_IT(&IMU_PORT, imu_rx_buffer, ONEBYTE);
				break;
		}
#endif
	}

	if(huart->Instance == DEBUG_PORT_ID)
	{
		debug_get_msg(debug_rx_buffer);
		HAL_UART_Receive_IT(&DEBUG_PORT, debug_rx_buffer, DEBUG_RX_BUFF_MAX);
	}
}


/*
 * 调试数据通信帧(输入数据为float类型)
 * |header|cmd|func|na|dt1_f1|dt1_f2|dt1_f3|dt1_f4|dt2_f1|dt2_f2|dt2_f3|dt2_f4|dt3_f1|dt3_f2|dt3_f3|dt3_f4|dt4_f1|dt4_f2|dt4_f3|dt4_f4|
*/
/**
* @brief
* @param
* @retval
*/
int usart2_init(void)
{
	return HAL_UART_Receive_IT(&DEBUG_PORT, debug_rx_buffer, DEBUG_RX_BUFF_MAX);
}


/**
* @brief
* @param
* @retval
*/
int usart4_init(void)
{
	return HAL_UART_Receive_IT(&IMU_PORT, imu_rx_buffer, ONEBYTE);
}


/**
  * @brief  Rx FIFO 1 message pending callback.
  * @param  hcan pointer to a CAN_HandleTypeDef structure that contains
  *         the configuration information for the specified CAN.
  * @retval None
  */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	uint8_t  data[8];
	HAL_StatusTypeDef	status;

	if (hcan == &CAN_PORT)
	{
		status = HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &Rx1Message, data);
		if (HAL_OK == status)
		{
#if 0
#if __CAN_EXT_ID__
			printf("Rx1Message.StdId is 0x%04lx\r\n",  Rx1Message.ExtId);
#else
			printf("Rx1Message.StdId is 0x%04lx\r\n",  Rx1Message.StdId);
#endif
			printf("data[0] is 0x%02x\r\n", data[0]);
			printf("data[1] is 0x%02x\r\n", data[1]);
			printf("data[2] is 0x%02x\r\n", data[2]);
			printf("data[3] is 0x%02x\r\n", data[3]);
			printf("data[4] is 0x%02x\r\n", data[4]);
			printf("data[5] is 0x%02x\r\n", data[5]);
			printf("data[6] is 0x%02x\r\n", data[6]);
			printf("data[7] is 0x%02x\r\n", data[7]);
//			printf("CAN1 <---\r\n");
#endif
//========== todo ==========
#if MOTOR_STEP42
			if(Rx1Message.StdId == 0x0A)
			{
				uint16_t tmp = data[2];
				tmp = (tmp<<8)+data[3];
				int dir = 1;
				switch(data[0])
				{
					case 1:
						// 这款电机的速度反馈没有方向，所以要对数据做方向处理
						if(xbot.mobile.motor[PHY_MOTOR_1].fPreSPD>=0)
						{
							dir = 1;
						}
						else
						{
							dir = -1;
						}
						xbot.mobile.motor[PHY_MOTOR_1].fActSPD = tmp*dir;
						xbot.mobile.motor[PHY_MOTOR_1].fActSPD = (xbot.mobile.motor[PHY_MOTOR_1].fActSPD*WHEEL_RADIUS)/10;
						break;
					case 2:
						// 这款电机的速度反馈没有方向，所以要对数据做方向处理
						if(xbot.mobile.motor[PHY_MOTOR_2].fPreSPD>=0)
						{
							dir = 1;
						}
						else
						{
							dir = -1;
						}
						xbot.mobile.motor[PHY_MOTOR_2].fActSPD = tmp*dir;
						xbot.mobile.motor[PHY_MOTOR_2].fActSPD = (xbot.mobile.motor[PHY_MOTOR_2].fActSPD*WHEEL_RADIUS)/10;
						break;
					case 3:
						// 这款电机的速度反馈没有方向，所以要对数据做方向处理
						if(xbot.mobile.motor[PHY_MOTOR_3].fPreSPD>=0)
						{
							dir = 1;
						}
						else
						{
							dir = -1;
						}
						xbot.mobile.motor[PHY_MOTOR_3].fActSPD = tmp*dir;
						xbot.mobile.motor[PHY_MOTOR_3].fActSPD = (xbot.mobile.motor[PHY_MOTOR_3].fActSPD*WHEEL_RADIUS)/10;
						break;
					case 4:
						// 这款电机的速度反馈没有方向，所以要对数据做方向处理
						if(xbot.mobile.motor[PHY_MOTOR_4].fPreSPD>=0)
						{
							dir = 1;
						}
						else
						{
							dir = -1;
						}
						xbot.mobile.motor[PHY_MOTOR_4].fActSPD = tmp*dir;
						xbot.mobile.motor[PHY_MOTOR_4].fActSPD = (xbot.mobile.motor[PHY_MOTOR_4].fActSPD*WHEEL_RADIUS)/10;
						break;
					default:
						break;
				}
			}
#endif
//========== end ==========
			Rx1Message.ExtId = 0;
			Rx1Message.StdId = 0;
		}
	}
}


/**
  * @brief
  * @param
  * @retval
  */
static void can_filter_config(void)
{
#if __CAN_1_ENB__
	CAN_FilterTypeDef  sFilterConfig1;
#endif
#if __CAN_2_ENB__
	CAN_FilterTypeDef  sFilterConfig2;
#endif

#if __CAN_1_ENB__
    sFilterConfig1.FilterBank = 0;
    sFilterConfig1.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig1.FilterScale = CAN_FILTERSCALE_32BIT;
    sFilterConfig1.FilterIdHigh = 0x0000;
    sFilterConfig1.FilterIdLow = 0x0000;
    sFilterConfig1.FilterMaskIdHigh = 0x0000;
    sFilterConfig1.FilterMaskIdLow = 0x0000;
    sFilterConfig1.FilterFIFOAssignment = CAN_RX_FIFO0;
    sFilterConfig1.FilterActivation = ENABLE;
    sFilterConfig1.SlaveStartFilterBank = 14;
    if (HAL_CAN_ConfigFilter(&CAN_PORT, &sFilterConfig1) != HAL_OK)
    {
    	printf("CAN Filter Config Fail!\r\n");
    }
	else
	{
		//printf("CAN Filter Config Success!\r\n");
	}
#endif

#if __CAN_2_ENB__
    sFilterConfig2.FilterBank = 14;
    sFilterConfig2.FilterMode = CAN_FILTERMODE_IDMASK;
    sFilterConfig2.FilterScale = CAN_FILTERSCALE_32BIT;
    sFilterConfig2.FilterIdHigh = 0x0000;
    sFilterConfig2.FilterIdLow = 0x0000;
    sFilterConfig2.FilterMaskIdHigh = 0x0000;
    sFilterConfig2.FilterMaskIdLow = 0x0000;
    sFilterConfig2.FilterFIFOAssignment = CAN_RX_FIFO1;
    sFilterConfig2.FilterActivation = ENABLE;
    sFilterConfig2.SlaveStartFilterBank = 14;
	if (HAL_CAN_ConfigFilter(&CAN_PORT_EXT, &sFilterConfig2) != HAL_OK)
	{
		printf("CAN Filter Config Fail!\r\n");
    }
	else
	{
		//printf("CAN Filter Config Success!\r\n");
	}
#endif
}

/**
  * @brief
  * @param
  * @retval
  */
void can_init(void)
{
	/* 1. CAN Filter Config */
	can_filter_config();

	/* 2. TX/RX ID Type Define*/
#if __CAN_1_ENB__
#if __CAN_EXT_ID__
	Tx1Message.IDE = CAN_ID_EXT;
	Tx1Message.RTR = CAN_RTR_DATA;

	Rx1Message.IDE = CAN_ID_EXT;
	Rx1Message.RTR = CAN_RTR_DATA;
#else
	Tx1Message.IDE = CAN_ID_STD;
	Tx1Message.RTR = CAN_RTR_DATA;

	Rx1Message.IDE = CAN_ID_STD;
	Rx1Message.RTR = CAN_RTR_DATA;
#endif
#endif

#if __CAN_2_ENB__
#if __CAN_EXT_ID__
	Tx2Message.IDE = CAN_ID_EXT;
	Tx2Message.RTR = CAN_RTR_DATA;֡

	Rx2Message.IDE = CAN_ID_EXT;
	Rx2Message.RTR = CAN_RTR_DATA;֡
#else
	Tx2Message.IDE = CAN_ID_STD;
	Tx2Message.RTR = CAN_RTR_DATA;֡

	Rx2Message.IDE = CAN_ID_STD;
	Rx2Message.RTR = CAN_RTR_DATA֡
#endif
#endif


  /* 3. CAN Start */
#if __CAN_1_ENB__
	if (HAL_CAN_Start(&CAN_PORT) != HAL_OK)
	{
		printf("CAN1 Start Fail\r\n");
	}
	else
	{
		//printf("CAN1 Start Success --BaudRate:(1M)\r\n");
	}
#endif

#if __CAN_2_ENB__
	if (HAL_CAN_Start(&CAN_PORT_EXT) != HAL_OK)
	{
		printf("CAN2 Start Fail\r\n");
	}
	else
	{
		//printf("CAN2 Start Success --BaudRate:(1M)\r\n");
	}
#endif

  /* 4. Enable CAN RX Interrupt */
#if __CAN_1_ENB__
	if (HAL_CAN_ActivateNotification(&CAN_PORT, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK)
	{
		printf("CAN_IT_RX_FIFO0_MSG_PENDING Enable Fail\r\n");
	}
#endif

#if __CAN_2_ENB__
	if (HAL_CAN_ActivateNotification(&CAN_PORT_EXT, CAN_IT_RX_FIFO1_MSG_PENDING) != HAL_OK)
	{
		printf("CAN_IT_RX_FIFO1_MSG_PENDING Enable Fail\r\n");
	}
#endif
}


/**
  * @brief
  * @param
  * @retval
  */
void can_set_msg(uint32_t can_id, uint8_t can_msg[8], uint8_t dlc)
{
#if __CAN_1_ENB__
#if __CAN_EXT_ID__
	Tx1Message.ExtId = can_id;
#else
	Tx1Message.StdId = can_id;
#endif
	Tx1Message.DLC = dlc;

	if(HAL_CAN_AddTxMessage(&CAN_PORT, &Tx1Message, can_msg, (uint32_t*)CAN_TX_MAILBOX0) != HAL_OK)
	{
		printf("CAN1 send data fail!\r\n");
	}
#endif
}

/**
  * @brief
  * @param
  * @retval
  */
void can_set_ext_msg(uint32_t can_id, uint8_t can_msg[8], uint8_t dlc)
{
#if __CAN_2_ENB__
#if __CAN_EXT_ID__
	Tx2Message.ExtId = can_id;
#else
	Tx2Message.StdId = can_id;
#endif
	Tx2Message.DLC = dlc;

	if(HAL_CAN_AddTxMessage(&CAN_PORT_EXT, &Tx2Message, can_msg, (uint32_t*)CAN_TX_MAILBOX1) != HAL_OK)
	{
		printf("CAN2 send data fail!\r\n");
	}
#endif
}

